#ifndef CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_
#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_

#include <deque>
#include <memory>
#include <iomanip>

#include "cartographer/common/time.h"
#include "cartographer/mapping/imu_tracker.h"
#include "cartographer/mapping/pose_extrapolator_interface.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/rcm/imu/pose_integrate.h"
#include "cartographer/rcm/imu/optimize.h"

namespace cartographer
{
    struct TimedPose
    {
        common::Time time;
        transform::Rigid3d pose;
    };

    inline PVR TimedPoseToPVR(const TimedPose &pose, const Eigen::Vector3d &linear_velocity)
    {
        PVR pvr;
        pvr.P = pose.pose.translation();
        pvr.R = pose.pose.rotation().normalized().toRotationMatrix();
        pvr.V = linear_velocity;
        pvr.time = pose.time;
        return pvr;
    }

    inline TimedPose PVRToTimedPose(const PVR &pose)
    {
        return TimedPose{pose.time, transform::Rigid3d(pose.P, Eigen::Quaterniond(pose.R))};
    }

    inline void PrintPose(const char *prefix, const TimedPose &pose)
    {
        const auto &trans = pose.pose.translation();
        const auto &rotation = QuatToEuler(pose.pose.rotation());
        LOG(INFO) << "\n"
                  << prefix << " trans:" << VecToStr(trans) << " rotat:" << VecToStr(rotation) << " time:" << common::ToUniversal(pose.time)
                  << "\n";
    }

    inline void PrintPose(const char *prefix, const transform::Rigid3d &pose)
    {
        const auto &trans = pose.translation();
        const auto &rotation = QuatToEuler(pose.rotation());
        LOG(INFO) << "\n" << prefix << " trans:" << VecToStr(trans) << " rotat:" << VecToStr(rotation) << "\n";
    }

    inline ImuOptNode::Ptr TimedPoseToImuOptNode(const TimedPose &pose, const Eigen::Vector3d &linear_velocity, const ImuBias &bias)
    {
        auto node = std::make_shared<ImuOptNode>();
        node->P = pose.pose.translation();
        node->Q = pose.pose.rotation().normalized();
        node->V = linear_velocity;
        node->Ba = bias.acc_;
        node->Bg = bias.gyro_;
        node->time = common::ToUniversal(pose.time);
        return node;
    }

    namespace mapping
    {
        class PoseExtrapolator : public PoseExtrapolatorInterface
        {
        public:
            explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant);

            PoseExtrapolator(const PoseExtrapolator &) = delete;
            PoseExtrapolator &operator=(const PoseExtrapolator &) = delete;

            static std::unique_ptr<PoseExtrapolator> InitializeWithImu(common::Duration pose_queue_duration,
                                                                       double imu_gravity_time_constant, const sensor::ImuData &imu_data);
            // 加入队列，裁剪(仅保持两个前端输出的位姿)
            // 更新imu_tracker_，目的是用两帧位姿求平均线速和角速
            // 用主ImuTracker同步其他两个ImuTracker;
            void AddPose(common::Time time, const transform::Rigid3d &pose) override;

            // 加入队列，裁剪，数据时间要在最后位姿的时间之后
            void AddImuData(const sensor::ImuData &imu_data) override;

            // 加入列队，裁剪，数据时间要在最后位姿的时间之后
            // 更新odometry_imu_tracker_
            // 目的是用两帧ODOM数据实时计算平均线速和角速
            void AddOdometryData(const sensor::OdometryData &odometry_data) override;

            void ScanMatchSuccess(const common::Time &time);

            transform::Rigid3d ExtrapolatePose(common::Time time) override;

            transform::Rigid3d ExtrapolateRobotPose(common::Time time) override;

            ExtrapolationResult ExtrapolatePosesWithGravity(const std::vector<common::Time> &times) override;

            inline common::Time GetLastPoseTime() const override
            {
                return timed_pose_queue_.empty() ? common::Time::min() : timed_pose_queue_.back().time;
            }

            inline common::Time GetLastExtrapolatedTime() const override
            {
                return !extrapolation_imu_tracker_ ? common::Time::min() : extrapolation_imu_tracker_->time();
            }

            inline Eigen::Vector3d GetLinearVelocity() const
            {
                return odometry_data_.size() < 2 ? linear_velocity_from_poses_ : linear_velocity_from_odometry_;
            }

            inline Eigen::Vector3d GetAngularVelocity() const
            {
                return odometry_data_.size() < 2 ? angular_velocity_from_poses_ : angular_velocity_from_odometry_;
            }

            // Returns the current gravity alignment estimate as a rotation from
            // the tracking frame into a gravity aligned frame.
            Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;

        private:
            void Optimize();
            void TrimImuData();
            void TrimOdometryData();
            void UpdateVelocitiesFromPoses();
            void TrimPose(const common::Time time);
            int ImuIntegrate(const TimedPose &lastPose, const common::Time currTime);
            void AdvanceImuTracker(common::Time time, ImuTracker *imu_tracker) const;
            void UpdateExtrapolatePose(const common::Time time);
            Eigen::Vector3d ExtrapolateTranslation(common::Time time);
            Eigen::Quaterniond ExtrapolateRotation(common::Time time, ImuTracker *imu_tracker) const;

            const common::Duration pose_queue_duration_;
            const double gravity_time_constant_;
            TimedPose cached_extrapolated_pose_;

            std::deque<TimedPose> timed_pose_queue_; // 只存两个pose
            Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
            Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();

            std::deque<sensor::ImuData> imu_data_;
            std::unique_ptr<ImuTracker> imu_tracker_;
            std::unique_ptr<ImuTracker> odometry_imu_tracker_;
            std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;

            ImuBias last_bias_;
            PoseIntegrator::Ptr poseIntegrator_;
            ImuIntegrateOptimizer::Ptr poseOptimizer_;
            std::deque<sensor::OdometryData> odometry_data_;
            Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
            Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
        };
    } // namespace mapping
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_
